/*
* Copyright (c) 2006-2007 Erin Catto http:
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked, and must not be
* misrepresented the original software.
* 3. This notice may not be removed or altered from any source distribution.
*
* Converted for The Render Engine v2.0
* Aug. 4, 2010 Brett Fattori
*/

Engine.include("/physics/common/b2Settings.js");
Engine.include("/physics/common/math/b2Vec2.js");

Engine.include("/physics/collision/b2Distance.js");

Engine.initObject("b2Conservative", null, function() {

   var b2Conservative = Base.extend({
      constructor: null,
      
      R1: new b2Mat22(),
      
      R2: new b2Mat22(),
      
      x1: new b2Vec2(),
      
      x2: new b2Vec2(),
      
      Conservative: function(shape1, shape2) {
         var body1 = shape1.GetBody();
         var body2 = shape2.GetBody();

         //b2Vec2 v1 = body1->m_position - body1->m_position0;
         var v1X = body1.m_position.x - body1.m_position0.x;
         var v1Y = body1.m_position.y - body1.m_position0.y;
         //float32 omega1 = body1->m_rotation - body1->m_rotation0;
         var omega1 = body1.m_rotation - body1.m_rotation0;
         //b2Vec2 v2 = body2->m_position - body2->m_position0;
         var v2X = body2.m_position.x - body2.m_position0.x;
         var v2Y = body2.m_position.y - body2.m_position0.y;
         //float32 omega2 = body2->m_rotation - body2->m_rotation0;
         var omega2 = body2.m_rotation - body2.m_rotation0;

         //float32 r1 = shape1->GetMaxRadius();
         var r1 = shape1.GetMaxRadius();
         //float32 r2 = shape2->GetMaxRadius();
         var r2 = shape2.GetMaxRadius();

         //b2Vec2 p1Start = body1->m_position0;
         var p1StartX = body1.m_position0.x;
         var p1StartY = body1.m_position0.y;
         //float32 a1Start = body1->m_rotation0;
         var a1Start = body1.m_rotation0;

         //b2Vec2 p2Start = body2->m_position0;
         var p2StartX = body2.m_position0.x;
         var p2StartY = body2.m_position0.y;
         //float32 a2Start = body2->m_rotation0;
         var a2Start = body2.m_rotation0;

         //b2Vec2 p1 = p1Start;
         var p1X = p1StartX;
         var p1Y = p1StartY;
         //float32 a1 = a1Start;
         var a1 = a1Start;
         //b2Vec2 p2 = p2Start;
         var p2X = p2StartX;
         var p2Y = p2StartY;
         //float32 a2 = a2Start;
         var a2 = a2Start;

         //b2Mat22 b2Conservative.R1(a1), b2Conservative.R2(a2);
         b2Conservative.R1.Set(a1);
         b2Conservative.R2.Set(a2);

         //shape1->QuickSync(p1, b2Conservative.R1);
         shape1.QuickSync(p1, b2Conservative.R1);
         //shape2->QuickSync(p2, b2Conservative.R2);
         shape2.QuickSync(p2, b2Conservative.R2);

         //float32 s1 = 0.0f;
         var s1 = 0.0;
         //const int32 maxIterations = 10;
         var maxIterations = 10;
         //b2Vec2 d;
         var dX;
         var dY;
         //float32 invRelativeVelocity = 0.0f;
         var invRelativeVelocity = 0.0;
         //bool hit = true;
         var hit = true;
         //b2Vec2 b2Conservative.x1, b2Conservative.x2; moved to static var
         for (var iter = 0; iter < maxIterations; ++iter)
         {
            // Get the accurate distance between shapes.
            //float32 distance = b2Distance.Distance(&b2Conservative.x1, &b2Conservative.x2, shape1, shape2);
            var distance = b2Distance.Distance(b2Conservative.x1, b2Conservative.x2, shape1, shape2);
            if (distance < b2Settings.b2_linearSlop)
            {
               if (iter == 0)
               {
                  hit = false;
               }
               else
               {
                  hit = true;
               }
               break;
            }

            if (iter == 0)
            {
               //b2Vec2 d = b2Conservative.x2 - b2Conservative.x1;
               dX = b2Conservative.x2.x - b2Conservative.x1.x;
               dY = b2Conservative.x2.y - b2Conservative.x1.y;
               //d.Normalize();
               var dLen = Math.sqrt(dX*dX + dY*dY);
               //float32 relativeVelocity = b2Dot(d, v1 - v2) + b2Abs(omega1) * r1 + b2Abs(omega2) * r2;
               var relativeVelocity = (dX*(v1X-v2X) + dY*(v1Y - v2Y)) + Math.abs(omega1) * r1 + Math.abs(omega2) * r2;
               if (Math.abs(relativeVelocity) < Number.MIN_VALUE)
               {
                  hit = false;
                  break;
               }

               invRelativeVelocity = 1.0 / relativeVelocity;
            }

            // Get the conservative movement.
            //float32 ds = distance * invRelativeVelocity;
            var ds = distance * invRelativeVelocity;
            //float32 s2 = s1 + ds;
            var s2 = s1 + ds;

            if (s2 < 0.0 || 1.0 < s2)
            {
               hit = false;
               break;
            }

            if (s2 < (1.0 + 100.0 * Number.MIN_VALUE) * s1)
            {
               hit = true;
               break;
            }

            s1 = s2;

            // Move forward conservatively.
            //p1 = p1Start + s1 * v1;
            p1X = p1StartX + s1 * v1.x;
            p1Y = p1StartY + s1 * v1.y;
            //a1 = a1Start + s1 * omega1;
            a1 = a1Start + s1 * omega1;
            //p2 = p2Start + s1 * v2;
            p2X = p2StartX + s1 * v2.x;
            p2Y = p2StartY + s1 * v2.y;
            //a2 = a2Start + s1 * omega2;
            a2 = a2Start + s1 * omega2;

            b2Conservative.R1.Set(a1);
            b2Conservative.R2.Set(a2);
            shape1.QuickSync(p1, b2Conservative.R1);
            shape2.QuickSync(p2, b2Conservative.R2);
         }

         if (hit)
         {
            // Hit, move bodies to safe position and re-sync shapes.
            //b2Vec2 d = b2Conservative.x2 - b2Conservative.x1;
            dX = b2Conservative.x2.x - b2Conservative.x1.x;
            dY = b2Conservative.x2.y - b2Conservative.x1.y;
            //float32 length = d.Length();
            var length = Math.sqrt(dX*dX + dY*dY);
            if (length > FLT_EPSILON)
            {
               d *= b2_linearSlop / length;
            }

            if (body1.IsStatic())
            {
               //body1.m_position = p1;
               body1.m_position.x = p1X;
               body1.m_position.y = p1Y;
            }
            else
            {
               //body1.m_position = p1 - d;
               body1.m_position.x = p1X - dX;
               body1.m_position.y = p1Y - dY;
            }
            body1.m_rotation = a1;
            body1.m_R.Set(a1);
            body1.QuickSyncShapes();

            if (body2.IsStatic())
            {
               //body2->m_position = p2;
               body2.m_position.x = p2X;
               body2.m_position.y = p2Y;
            }
            else
            {
               //body2->m_position = p2 + d;
               body2.m_position.x = p2X + dX;
               body2.m_position.y = p2Y + dY;
            }
            //body2.m_position = p2 + d;
            body2.m_position.x = p2X + dX;
            body2.m_position.y = p2Y + dY;
            body2.m_rotation = a2;
            body2.m_R.Set(a2);
            body2.QuickSyncShapes();

            return true;
         }

         // No hit, restore shapes.
         shape1.QuickSync(body1.m_position, body1.m_R);
         shape2.QuickSync(body2.m_position, body2.m_R);
         return false;
      }
      
   });
   
   return b2Conservative;

});
